#include "pcloud_scans/line_extractor.h"
#include <list>
#include <math.h>
// line extractor base on split algorithm
 /*int MINIMUM_POINTS_CHECK = 20;//点云数目
 int MINIMUM_INDEX = 15;//线点数目
 double MAXIMUM_GAP_DISTANCE = 0.2;//点之间的距离 
 double IN_LINE_THRESHOLD = 0.015;//描述线的曲率*/


void lineExtractor(const std::vector<Point> data,std::vector<Line>& real_line,int LINE_MINIMUM_POINTS,int LINE_MINIMUM_INDEX,double LINT_AP_DISTANCE,double LINE_DIS_THRESHOLD,double LINE_LENGTH)
{
 int MINIMUM_POINTS_CHECK = LINE_MINIMUM_POINTS;
 int MINIMUM_INDEX = LINE_MINIMUM_INDEX;
 double MAXIMUM_GAP_DISTANCE = LINT_AP_DISTANCE;
 double IN_LINE_THRESHOLD = LINE_DIS_THRESHOLD;

  if (data.size() < MINIMUM_POINTS_CHECK) return;
  // 1#: we initial a line from start to end
  //-----------------------------------------
  Point start = data.front();
  Point end = data.back();
  Line l;
  l.start_index = 0;
  l.end_index = data.size() - 1;
  std::list<Line> line_list;
  line_list.push_back(l);
/////////////////////////////////////////////////////////////////////////////////////////////////////
  while (!line_list.empty()) 
  {
    // 2#: everytime we take the first line in
    //line list to check if every point is on this line
    //-----------------------------------------
    Line& lr = *line_list.begin();
    if (lr.end_index - lr.start_index < MINIMUM_INDEX || lr.start_index == lr.end_index)// || sqrt(I*I + J*J<0.01))
    {
      line_list.pop_front();//删除line_list第一条线
      continue;
    }

    // 3#: use two points to generate a line equation  使用两点生成直线方程
    //-----------------------------------------
    start.x = data[lr.start_index].x;
    start.y = data[lr.start_index].y;
    end.x = data[lr.end_index].x;
    end.y = data[lr.end_index].y;

    // two points P1(x1, y1), P2(x2,y2) are given, and these two points are not the same
    // we can calculate an equation to model a line these two points are on.
    double A = end.y - start.y;
    double B = start.x - end.x;
    double C = end.x * start.y - start.x * end.y;

    double max_distance = 0;
    int max_i;
    int gap_i(-1);
    // the kernel code
    for (int i = lr.start_index + 1; i <= lr.end_index - 1; i++) 
    {
      // 4#: if two points' distance is too large, it's meaningless to generate a lineconnects these two points, so we have to filter it.
     // 如果两点之间的距离太大，生成一条连接这两点的线是没有意义的，所以我们必须对其进行过滤。
      //-----------------------------------------
      double point_gap_dist = hypot(data[i].x - data[i+1].x, data[i].y - data[i+1].y);//hypot计算斜边长度
      if (point_gap_dist > MAXIMUM_GAP_DISTANCE) 
      {
        gap_i = i;
        break;
      }

      // 5#: calculate the distance between every point to the line
      //-----------------------------------------
      double dist = fabs(A * data[i].x + B * data[i].y + C) / hypot(A, B);//点到线的距离
      if (dist > max_distance) {
        max_distance = dist;
        max_i = i;
      }
    }

    // 6#: if gap is too large or there's a point is far from the line,
    // we have to split this line to two line, then check again.
    //-----------------------------------------
    if (gap_i != -1) //两点之间的距离太大
    {
      int tmp = lr.end_index;
      lr.end_index = gap_i;
      Line ll;
      ll.start_index = gap_i + 2;   //分成二段线，循环
      ll.end_index = tmp;//最后一个点
      line_list.insert(++line_list.begin(), ll);
    }
    else if (max_distance > IN_LINE_THRESHOLD) //点到线的距离太大
    {
      int tmp = lr.end_index;
      lr.end_index = max_i;
      Line ll;
      ll.start_index = max_i + 2;
      ll.end_index = tmp;//最后一个点
      line_list.insert(++line_list.begin(), ll);
    } 
    else //都符合 获取线
    {
      double I = data[lr.end_index].x - data[lr.start_index].x ;
      double J = data[lr.end_index].y - data[lr.start_index].y;
      if(sqrt(I*I+J*J)>=LINE_LENGTH)
      {
        real_line.push_back(line_list.front());//从第一条里面要找到一条直线（长度不够），才把第一条删除，然后再从第二条再次循环
        double temp = real_line[real_line.size()-1].end_index;
        real_line[real_line.size()-1].end_index = real_line[real_line.size()-1].start_index;
        real_line[real_line.size()-1].start_index = temp;
      }
      line_list.pop_front();//删除该条线   
    }
  }
}
